#include <iostream>
#include <unistd.h>
#include <ctime>
#include <math.h>
#include <QPainter>
#include <QPainterPath>
#include <QPixmap>

#include "MWiget.h"
#include "cereSolve.h"
#include "util.h"

using namespace std;

MWiget::MWiget(QWidget *parent) :
    QWidget(parent),R(160),centerX(320),centerY(240),dataNum(60),isUpdated(false)
{
    init();
    setFixedSize(640,480);
}

int MWiget::init()
{
    std::vector<double> data;
    generatorData(dataNum,data,centerX,centerY,R);

    for (int index = 0; index < dataNum; ++index)
    {
        QPointF point;
        point.setX(data[index*2]);
        point.setY(data[index*2+1]);
        QPointData.push_back(point);
    }
    return 0;
}

void MWiget::paintEvent(QPaintEvent *)
{
    QPainter painter(this);
    QPen pen;
    pen.setWidth(3);
    pen.setColor(Qt::red);
    painter.setPen(pen);
    for(auto item : QPointData)
        painter.drawPoint(item);

    if(isUpdated)
    {
        painter.setPen(QPen(Qt::blue,4,Qt::SolidLine));//设置画笔形式
        painter.setBrush(QBrush(Qt::red,Qt::SolidPattern));//设置画刷形式
        painter.drawEllipse(point0,6,6);
        painter.drawEllipse(point1,6,6);
        painter.drawEllipse(point2,6,6);
        painter.drawLine(point0,point1);
        painter.drawLine(point0,point2);
        painter.setPen(QPen(Qt::yellow,4,Qt::DotLine));
        painter.drawRect(point01.x()-2,point01.y()-2,4,4);
        painter.drawRect(point02.x()-2,point02.y()-2,4,4);

        if(fabs(first.getB())>1e-3){
            QPointF firstPoint(this->width(),-(first.getA()*this->width()+first.getC())/first.getB());
            QPointF secondPoint(0.f,-(first.getC())/first.getB());
            painter.drawLine(firstPoint,secondPoint);
        }
        if(fabs(second.getB())>1e-3){
            QPointF firstPoint(this->width(),-(second.getA()*this->width()+second.getC())/second.getB());
            QPointF secondPoint(1.f,-(second.getA()+second.getC())/second.getB());
            painter.drawLine(firstPoint,secondPoint);
        }

        painter.setPen(QPen(Qt::green,4,Qt::SolidLine));
        painter.drawEllipse(centerResult,2,2);
        painter.setPen(QPen(Qt::lightGray,2,Qt::SolidLine));
        painter.setBrush(QBrush());
        painter.drawEllipse(centerResult,R_Result-8,R_Result-8);
        painter.drawEllipse(centerResult,R_Result+8,R_Result+8);

        QImage imageObs(size(), QImage::Format_RGB32);
        QPainter painter2(&imageObs);
        render(&painter2);
        imageObs.save("layer.png",0,100);
        isUpdated = false;
    }
}

void MWiget::keyPressEvent(QKeyEvent *event)
{
    switch (event->key()) {
    case Qt::Key_Escape:
    {
        close();
        break;
    }
    case Qt::Key_0:
    {
        solve();
        break;
    }
    case Qt::Key_1:
    {
        Line first(QPointF(0.,1),QPointF(0.,2));
        Line second(QPointF(1,0.),QPointF(-2,0.));
        QPointF cross = first.cross(second);
        cout << cross.x() << ", " << cross.y() << std::endl;
        Line other(QPointF(0.,1),QPointF(0.,10));
        cross = first.cross(other);
        cout << cross.x() << ", " << cross.y() << std::endl;
    }
    default:
        break;
    }
}

int MWiget::solve()
{
    const int MaxIter = 30;
    srand((unsigned)time(0));
    bool solved = false;
    while (!solved)
    {
        int Index_0 = rand() % dataNum;
        int Index_1 = rand() % dataNum;
        int Index_2 = rand() % dataNum;

        point0 = QPointF(QPointData[Index_0]);
        point1 = QPointF(QPointData[Index_1]);
        point2 = QPointF(QPointData[Index_2]);

        //!< stage 1
        //! remove some too closed points and almost on one line;
        double xDiff01 = point1.x() - point0.x();
        double xDiff02 = point2.x() - point0.x();
        double yDiff01 = point1.y() - point0.y();
        double yDiff02 = point2.y() - point0.y();

        if((fabs(xDiff01)<8 && fabs(yDiff01)<8) || (fabs(xDiff02)<8 && fabs(yDiff02)<8))
            continue;

        double dotSum = xDiff01*xDiff02 + yDiff01*yDiff02;
        double normalLength = sqrt(xDiff01*xDiff01 + yDiff01*yDiff01)
                            * sqrt(xDiff02*xDiff02 + yDiff02*yDiff02);
        if(fabs(dotSum/normalLength)>0.9){
            continue;
        }

        //!< stage 2
        //! ransac filter:
        point01 = QPointF((point0.x()+point1.x())/2.0,(point0.y()+point1.y())/2.0);
        point02 = QPointF((point0.x()+point2.x())/2.0,(point0.y()+point2.y())/2.0);

        first = Line(point1.x()-point0.x(),point1.y()-point0.y(),point01);
        second = Line(point2.x()-point0.x(),point2.y()-point0.y(),point02);
        centerResult = first.cross(second);

        double xDiff = point0.x()-centerResult.x();
        double yDiff = point0.y()-centerResult.y();
        R_Result = sqrt(xDiff*xDiff + yDiff*yDiff);

        vector<int> candidateIndex;
        for (int var = 0; var < dataNum; ++var) {
            double pointDiffX = QPointData[var].x() - centerResult.x();
            double pointDiffY = QPointData[var].y() - centerResult.y();
            double distance = sqrt(pointDiffX*pointDiffX + pointDiffY*pointDiffY) - R_Result;
            if(fabs(distance)<8)
                candidateIndex.push_back(var);
        }
        if(candidateIndex.size()<dataNum*0.55){
            cout << "only have: " << candidateIndex.size() << " inlines!" << endl;
            continue;
        }
        else
        {
            cout << "Yes, have: " << candidateIndex.size() << " inlines!" << endl;
            //!< stage 3. least-square optimation
            QVector<QPointF> dataInliner;
            for (int i = 0; i < candidateIndex.size(); ++i) {
                dataInliner.push_back(QPointData[candidateIndex[i]]);
            }
            double parameter[3] = {centerResult.x(),centerResult.y(),R_Result};
            solveCircleFitting(dataInliner,parameter);
            centerResult.setX(parameter[0]);
            centerResult.setY(parameter[1]);
            R_Result = parameter[2];
            cout << centerResult.x() << " " << centerResult.y() << " " << R_Result << "\n";
        }

        isUpdated = true;
        repaint();

        sleep(1);
        solved = true;
    }
}
